package edu.wpi.first.wpilibj.robot;
import edu.wpi.first.wpilibj.*;

public class GunMotor extends Motor{
    Joystick joy;
    DriveMotor dm;
    BeltMotor bm;
    
    Keybinds kb;
    PrintMotor pm;
    double wheelSpeed = .4;
    
    boolean increaseButton = false;
    boolean decreaseButton = false;
    
    public GunMotor(Joystick joy, Jaguar[] duper, DriveMotor dm, BeltMotor bm, Keybinds kb, PrintMotor pm){
        super(duper);
        this.joy = joy;
        this.dm = dm;
        this.bm = bm;
        this.kb = kb;
        this.pm = pm;
        
        /**
         * jaguar[0] is the trigger jag powered by the servo motor that ONLY comes on while the trigger is held
         * 
         * jaguar[1] is the big servo guy who actually shoots the ball by pushing it into that plate.
         */
    }
    public GunMotor(Joystick joy, Jaguar[] duper, DriveMotor dm, BeltMotor bm, PrintMotor pm){
        this(joy, duper, dm, bm, new Keybinds(), pm);
    }
    
    public void run(){
        while (true){
            Jaguar launcherJag = theJags[1];
            Jaguar triggerJag = theJags[0];
            
            
            boolean increasing = joy.getRawButton(kb.increaseSpeed);
            boolean decreasing = joy.getRawButton(kb.lowerSpeed);
            if (increasing!=increaseButton && increasing){
                increaseSpeed();
            }
            else if (decreasing!=decreaseButton && decreasing){
                decreaseSpeed();
            }
            decreaseButton = decreasing;
            increaseButton = increasing;
                
                
                
            launcherJag.set(wheelSpeed);
            
            while (joy.getRawButton(kb.FIRE)){
                dm.shooting = true;
                triggerJag.set(-.3);
            }
            
            
            
            bm.shooting = false;
            dm.shooting = false;
            theJags[0].set(0);
        }
    }
    
    public void setSpeed(double speed){
        if (speed<0 || speed>1)return;
        while((wheelSpeed-speed)*(wheelSpeed-speed) > .01){
            if (wheelSpeed-speed > 0) decreaseSpeed();
            if (wheelSpeed-speed < 0) increaseSpeed();
        }
    }
    
    public void increaseSpeed(){
        wheelSpeed += .1;
        if (wheelSpeed>1) wheelSpeed = 1;
    }
    
    public void decreaseSpeed(){
        wheelSpeed -= .1;
        if (wheelSpeed<0) wheelSpeed = 0;
    }
}
